#include "DofSample.h"
#include "moremath.h"

DofSample::DofSample(int _index){
	mStartIndex=_index;
	mCtrlPts.clear();
	mNumPts=0;
}

DofSample::DofSample(int _index, double nfstart, double nfend){
	mStartIndex=_index;
	mCtrlPts.clear();
	mCtrlPts.resize(nfend-nfstart+1);
	for(int i=0; i<mCtrlPts.size(); i++) {
		mCtrlPts[i][0]=nfstart+i;
		mCtrlPts[i][1]=0;
	}
	mNumPts=mCtrlPts.size();
}

inline double DofSample::getValue(double _frame){
//	assert(mCtrlPts[_frame][0]==_frame);
	return mCtrlPts[_frame][1];
}

inline double DofSample::getSlope(double _frame){
	int intFrame = (int)moremath::round(_frame);
	return (mCtrlPts[intFrame][1] - mCtrlPts[intFrame-1][1]) / (mCtrlPts[intFrame][0] - mCtrlPts[intFrame-1][0]);
}

inline double DofSample::getCtrlPtCoord(int _numC){
	return mCtrlPts[_numC][0];
}

inline double DofSample::getCtrlPtValue(int _numC){
	return mCtrlPts[_numC][1];
}

void DofSample::setCtrlPtValue(int _numC, double _v){
	mCtrlPts[_numC][1] = _v;
}

inline void DofSample::getCtrlPts(double *state){
	for(int i=0; i<mNumPts; i++){
		state[mStartIndex+i] = mCtrlPts[i][1];
	}
}

void DofSample::getCtrlPts(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		state[mStartIndex + i] = mCtrlPts[i][1];
	}
}

inline void DofSample::setCtrlPts(double *state){
	for(int i=0; i<mNumPts; i++){
		mCtrlPts[i][1] = state[mStartIndex+i];
	}
}

void DofSample::setCtrlPts(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		mCtrlPts[i][1] = state[mStartIndex+i];
	}
}

void DofSample::addCtrlPtsDiff(double *state, int _start, int _end){
	for(int i=_start; i<=_end; i++){
		mCtrlPts[i][1] += state[mStartIndex+i];
	}
}

void DofSample::initValues(vector<double> q){
	assert(q.size()==mNumPts);
	for(int i=0; i<mNumPts; i++) {
		mCtrlPts[i][1] = q[i];
	}
}

void DofSample::setValue(double _frame, double val){
	mCtrlPts[_frame][1] = val;
}

double DofSample::getDerivative(double _frame, int npt){
	assert(npt==0);
	return 1;
}

double DofSample::getSlopeDerivative(double _frame, int npt){
	assert(npt==0);
	if(npt==0) return -1;
	else if(npt==1) return 1;
	else {
		printf("DofSample::getVelDerivative - wrong crtl pt index passed for derivative\n");
		exit(1);
	}
	return 0;
}

double DofSample::getSecondDerivative(double _frame, int npt){
	return 0;
}

void DofSample::getCtrlPtIndices(double _frame, int *npts, vector<int> *indices){
	indices->clear();
	*npts=1;
	indices->push_back(mStartIndex+_frame);
}

void DofSample::getVelCtrlPtIndices(double _frame, int *npts, vector<int> *indices){
	indices->clear();
	*npts=2;
	indices->push_back(mStartIndex + _frame-1);
	indices->push_back(mStartIndex + _frame);
}
